
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mode_loiter.c
  * @author     baiyang
  * @date       2022-2-27
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "mode.h"
#include "fms.h"
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static bool mloiter_init(mode_base_t mode, bool ignore_checks);
static void mloiter_run(mode_base_t mode);

static ModeNumber mloiter_mode_number(mode_base_t mode);
static bool mloiter_requires_GPS(mode_base_t mode);
static bool mloiter_has_manual_throttle(mode_base_t mode);
static bool mloiter_allows_arming(mode_base_t mode, ArmingMethod method);
static bool mloiter_is_autopilot(mode_base_t mode);
static bool mloiter_has_user_takeoff(mode_base_t mode, bool must_navigate);
static bool mloiter_allows_autotune(mode_base_t mode);

static const char *mloiter_name(mode_base_t mode);
static const char *mloiter_name4(mode_base_t mode);

static uint32_t mloiter_wp_distance(mode_base_t mode);
static int32_t mloiter_wp_bearing(mode_base_t mode);
static float mloiter_crosstrack_error(mode_base_t mode);

/*----------------------------------variable----------------------------------*/
static struct mode_ops mode_loiter_ops = {
        .mode_number = mloiter_mode_number,
        .init        = mloiter_init,
        .exit        = NULL,
        .run         = mloiter_run,
        .requires_GPS = mloiter_requires_GPS,
        .has_manual_throttle = mloiter_has_manual_throttle,
        .allows_arming = mloiter_allows_arming,
        .is_autopilot = mloiter_is_autopilot,
        .has_user_takeoff = mloiter_has_user_takeoff,
        .in_guided_mode = NULL,
        .logs_attitude = NULL,
        .allows_save_trim = NULL,
        .allows_autotune = mloiter_allows_autotune,
        .allows_flip = NULL,
        .name = mloiter_name,
        .name4 = mloiter_name4,
        .is_taking_off = NULL,
        .is_landing = NULL,
        .requires_terrain_failsafe = NULL,
        .get_wp = NULL,
        .wp_bearing = mloiter_wp_bearing,
        .wp_distance = mloiter_wp_distance,
        .crosstrack_error = mloiter_crosstrack_error,
        .output_to_motors = NULL,
        .use_pilot_yaw = NULL,
        .throttle_hover = NULL,
        .do_user_takeoff_start = NULL,
        .pause = NULL,
        .resume = NULL};
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/// ModeLoiter
void mode_loiter_ctor(ModeLoiter* mode_loiter)
{
    mode_ctor(&mode_loiter->mode, &mode_loiter_ops);
}

static bool mloiter_init(mode_base_t mode, bool ignore_checks)
{
    if (!fms.failsafe.radio) {
        float target_roll, target_pitch;
        // apply SIMPLE mode transform to pilot inputs
        //update_simple_mode();

        // convert pilot input to lean angles
        fms_get_pilot_desired_lean_angles(&target_roll, &target_pitch, loiter_get_angle_max_cd(mode->loiter_nav), attctrl_get_althold_lean_angle_max_cd(mode->attitude_control));

        // process pilot's roll and pitch input
        loiter_set_pilot_desired_acceleration(mode->loiter_nav, target_roll, target_pitch);
    } else {
        // clear out pilot desired acceleration in case radio failsafe event occurs and we do not switch to RTL for some reason
        loiter_clear_pilot_desired_acceleration(mode->loiter_nav);
    }

    loiter_init_target(mode->loiter_nav);

    // initialise the vertical position controller
    if (!posctrl_is_active_z(mode->pos_control)) {
        posctrl_init_z_controller(mode->pos_control);
    }

    // set vertical speed and acceleration limits
    posctrl_set_max_speed_accel_z(mode->pos_control, -fms_get_pilot_speed_dn(), fms.g.pilot_speed_up, fms.g.pilot_accel_z);
    posctrl_set_correction_speed_accel_z(mode->pos_control, -fms_get_pilot_speed_dn(), fms.g.pilot_speed_up, fms.g.pilot_accel_z);

    return true;

}
static void mloiter_run(mode_base_t mode)
{
    float target_roll, target_pitch;
    float target_yaw_rate = 0.0f;
    float target_climb_rate = 0.0f;

    // set vertical speed and acceleration limits
    posctrl_set_max_speed_accel_z(mode->pos_control, -fms_get_pilot_speed_dn(), fms.g.pilot_speed_up, fms.g.pilot_accel_z);

    // process pilot inputs unless we are in radio failsafe
    if (!fms.failsafe.radio) {
        // apply SIMPLE mode transform to pilot inputs
        //update_simple_mode();

        // convert pilot input to lean angles
        fms_get_pilot_desired_lean_angles(&target_roll, &target_pitch, loiter_get_angle_max_cd(mode->loiter_nav), attctrl_get_althold_lean_angle_max_cd(mode->attitude_control));

        // process pilot's roll and pitch input
        loiter_set_pilot_desired_acceleration(mode->loiter_nav, target_roll, target_pitch);

        // get pilot's desired yaw rate
        target_yaw_rate = fms_get_pilot_desired_yaw_rate(RC_norm_input_dz(mode->channel_yaw));

        // get pilot desired climb rate
        target_climb_rate = fms_get_pilot_desired_climb_rate(mode->channel_throttle->control_in);
        target_climb_rate = math_constrain_float(target_climb_rate, -fms_get_pilot_speed_dn(), fms.g.pilot_speed_up);
    } else {
        // clear out pilot desired acceleration in case radio failsafe event occurs and we do not switch to RTL for some reason
        loiter_clear_pilot_desired_acceleration(mode->loiter_nav);
    }

    // relax loiter target if we might be landed
    if (fms.ap.land_complete_maybe) {
        loiter_soften_for_landing(mode->loiter_nav);
    }

    // Loiter State Machine Determination
    AltHoldModeState loiter_state = mode_get_alt_hold_state(target_climb_rate);

    // Loiter State Machine
    switch (loiter_state) {

    case AltHold_MotorStopped: {
        attctrl_reset_rate_controller_I_terms(mode->attitude_control);
        attctrl_reset_yaw_target_and_rate(mode->attitude_control, true);
        posctrl_relax_z_controller(mode->pos_control, 0.0f);   // forces throttle output to decay to zero
        loiter_init_target(mode->loiter_nav);
        Vector3f_t thrust_vector = loiter_get_thrust_vector(mode->loiter_nav);
        attctrl_input_thrust_vector_rate_heading(mode->attitude_control, &thrust_vector, target_yaw_rate);
        } break;

    case AltHold_Takeoff: {
        // initiate take-off
        if (!takeoff_running(mode->takeoff)) {
            takeoff_start(mode->takeoff, math_constrain_float(fms.g.pilot_takeoff_alt,0.0f,1000.0f));
        }

        // get avoidance adjusted climb rate
        //target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);

        // set position controller targets adjusted for pilot input
        takeoff_do_pilot_takeoff(mode->takeoff, target_climb_rate);

        // run loiter controller
        loiter_update(mode->loiter_nav, true);

        Vector3f_t thrust_vector = loiter_get_thrust_vector(mode->loiter_nav);

        // call attitude controller
        attctrl_input_thrust_vector_rate_heading(mode->attitude_control, &thrust_vector, target_yaw_rate);
        } break;

    case AltHold_Landed_Ground_Idle:
        attctrl_reset_yaw_target_and_rate(mode->attitude_control, true);
    case AltHold_Landed_Pre_Takeoff: {
        attctrl_reset_rate_controller_I_terms_smoothly(mode->attitude_control);
        loiter_init_target(mode->loiter_nav);

        Vector3f_t thrust_vector = loiter_get_thrust_vector(mode->loiter_nav);

        // call attitude controller
        attctrl_input_thrust_vector_rate_heading(mode->attitude_control, &thrust_vector, target_yaw_rate);
        posctrl_relax_z_controller(mode->pos_control, 0.0f);   // forces throttle output to decay to zero
        } break;

    case AltHold_Flying: {
        // set motors to full range
        Motors_set_desired_spool_state(fms.motors, MOTOR_DESIRED_THROTTLE_UNLIMITED);

        // run loiter controller
        loiter_update(mode->loiter_nav, true);

        Vector3f_t thrust_vector = loiter_get_thrust_vector(mode->loiter_nav);

        // call attitude controller
        attctrl_input_thrust_vector_rate_heading(mode->attitude_control, &thrust_vector, target_yaw_rate);

        // get avoidance adjusted climb rate
        //target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);

        // update the vertical offset based on the surface measurement
        //copter.surface_tracking.update_surface_offset();

        // Send the commanded climb rate to the position controller
        posctrl_set_pos_target_z_from_climb_rate_cm(mode->pos_control, target_climb_rate);
        } break;
    }

    // run the vertical position controller and set output throttle
    posctrl_update_z_controller(mode->pos_control);
}

static ModeNumber mloiter_mode_number(mode_base_t mode) { return LOITER; }
static bool mloiter_requires_GPS(mode_base_t mode) { return true; }
static bool mloiter_has_manual_throttle(mode_base_t mode) { return false; }
static bool mloiter_allows_arming(mode_base_t mode, ArmingMethod method) { return true; };
static bool mloiter_is_autopilot(mode_base_t mode) { return false; }
static bool mloiter_has_user_takeoff(mode_base_t mode, bool must_navigate) { return true; }
static bool mloiter_allows_autotune(mode_base_t mode) { return true; }

static const char *mloiter_name(mode_base_t mode) { return "LOITER"; }
static const char *mloiter_name4(mode_base_t mode) { return "LOIT"; }

static uint32_t mloiter_wp_distance(mode_base_t mode) { return loiter_get_distance_to_target(mode->loiter_nav);}
static int32_t mloiter_wp_bearing(mode_base_t mode) { return loiter_get_bearing_to_target(mode->loiter_nav);}
static float mloiter_crosstrack_error(mode_base_t mode) { return posctrl_crosstrack_error(mode->pos_control);}

/*------------------------------------test------------------------------------*/


